#include "../_includes.h"

bool running = false;
void print() {
  printf("data=[\n");
  int k = 0;
  while (running) {
    k++;
    printf("[%3d,%5.2f,%5.2f,%7.4f],\n", 10 * k, get_gyro(), get_gyro_rate(),
           chassis_l.get_set_power_pct());
    wait(10);
  }
  printf("]\n");
}

////////////////////////////
#define MAX_TURNING_SPEED 600 // dps 度每秒
// dps^2度每平方秒(°/s^2)---在直接设置微小负电压情况下直接滑动摩擦降速
#define MAX_DECELERATING_RATE 2380 
void turn_physical(double aim_angle, double max_power, int timeout) {

  Filter filter_acc;
  running = true;
  thread tt(print);
  wait(10);

  gyro_reset();
  chassis.FT_set_speed(0, 100);
  double rate_last = 0;
  double acc_recently, vt, st, min_decelerating_angle, angle_now, rate_now;
  while (1) {
    angle_now = get_gyro();
    rate_now = get_gyro_rate();
    acc_recently = filter_acc.filtered_value((rate_now - rate_last) / 0.01);

    vt = rate_now + acc_recently * 0.04; //未来40ms
    st = angle_now + (rate_now + vt) * 0.04 / 2;

    min_decelerating_angle = vt * vt / 2 / MAX_DECELERATING_RATE; //最短刹停角度

    if (st + min_decelerating_angle > aim_angle) {
      chassis.FT_set_voltage(0, -5);
      while (get_gyro_rate() > 0)
        wait(5);
      break;
    }
    wait(10);
    rate_last = rate_now;
  }
  chassis.stop();
  running = false;
  wait(50);
  printf("# %.2f  %.2f  %.2f %.2f  %.2f\n", angle_now, acc_recently, vt, st,
         min_decelerating_angle);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////
